﻿#include "floodfill_mdf.h"
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <list>
#include <set>
/*!
 * \brief mdf_rev_fill 反向搜索，从终点处找起点
 * \param v_mat   障碍地形，1是平地，0是墙
 * \param startx  开始位置x
 * \param starty  开始位置y
 * \param endx    结束位置x
 * \param endy    结束位置y
 * \param p_rev   保存搜索步长的v_mat等尺寸矩阵
 * \return 能否找到起点
 */
bool mdf_rev_fill(const std::vector<std::vector<char> > &v_mat,
				  const int startx,
				  const int starty,
				  const int endx,
				  const int endy,
				  std::vector<std::vector<unsigned int> > *p_rev)
{
	std::vector<std::vector<unsigned int> > &v_rev = *p_rev;
	p_rev->clear();
	const int rows = v_mat.size();
	const int cols = v_mat[0].size();
	for (int i = 0; i < rows; ++i)
	{
		std::vector<unsigned int> row;
		row.resize(cols, 0xffffffffu);
		p_rev->push_back(std::move(row));
	}
	//反向着色
	std::list<int> currX, currY;
	unsigned int step = 1;
	currX.push_back(endx);
	currY.push_back(endy);
	v_rev[endy][endx] = 0;
	bool arrival = false;
	while (currX.size() && !arrival)
	{
		const int rev_dirt[4][2] = {{1, 0}, {0, 1}, {-1, 0}, {0, -1}};
		const int tasks = currX.size();
		for (int t = 0; t < tasks && !arrival; ++t)
		{
			const int cx = *currX.begin();
			const int cy = *currY.begin();
			currX.pop_front();
			currY.pop_front();
			for (int i = 0; i < 4 && !arrival; ++i)
			{
				const int nx = cx + rev_dirt[i][0];
				const int ny = cy + rev_dirt[i][1];
				if (nx == startx && ny == starty)
					arrival = true;
				if (nx >= cols || nx < 0)
					continue;
				if (ny >= rows || ny < 0)
					continue;
				if (v_mat[ny][nx] == 0)
					continue;
				if (v_rev[ny][nx] < 0xffffffffu)
					continue;
				v_rev[ny][nx] = step;
				currX.push_back(nx);
				currY.push_back(ny);
			} //end for (int i=0;i<4&& !arrival;++i)
		} //end for (int t = 0; t< tasks&& !arrival; ++t)
		++step;
	} //end while (currX.size() && !arrival)
	return arrival;
}
/*!
 * \brief mdf_path_find 前向搜索路径
 * \param v_rev   反向寻找后生成的距离矩阵
 * \param startx  起点x
 * \param starty  起点y
 * \param cx      存储路径坐标的x向量
 * \param cy      存储路径坐标的y向量
 * \return  是否成功搜索生成路径
 */
bool mdf_path_find(std::vector<std::vector<unsigned int> > &v_rev,
				   const int startx,
				   const int starty,
				   std::vector<int> *cx,
				   std::vector<int> *cy)
{
	const int rows = v_rev.size();
	const int cols = v_rev[0].size();
	unsigned int v = v_rev[starty][startx];
	cx->clear();
	cy->clear();
	cx->push_back(startx);
	cy->push_back(starty);
	int tx = startx, ty = starty;
	v_rev[ty][tx] = 0xffffffffu;
	while (v > 0 && v < 0xffffffffu)
	{
		const int rev_dirt[8][2]
			= {{1, 0}, {0, 1}, {-1, 0}, {0, -1}, {1, 1}, {-1, 1}, {1, -1}, {-1, -1}};
		unsigned int minv = 0xffffffffu;
		int mind = 0;
		for (int i = 0; i < 8; ++i)
		{
			const int nx = tx + rev_dirt[i][0];
			const int ny = ty + rev_dirt[i][1];
			if (nx >= cols || nx < 0)
				continue;
			if (ny >= rows || ny < 0)
				continue;
			if (v_rev[ny][nx] < minv)
			{
				minv = v_rev[ny][nx];
				mind = i;
			}
		}
		v = minv;
		tx += rev_dirt[mind][0];
		ty += rev_dirt[mind][1];
		cx->push_back(tx);
		cy->push_back(ty);
		assert(tx >= 0 && tx < cols);
		assert(ty >= 0 && ty < rows);
	}
	return (v == 0);
}

/*!
 * \brief min_dis_opt 归并路径缩短距离
 * \param v_mat 障碍地形，1是平地，0是墙
 * \param rx 非最优路径x（直接搜索出来的）
 * \param ry 非最优路径y（直接搜索出来的）
 * \param cx 较优路径x
 * \param cy 较优路径y
 * \param pidx 关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
 * \param safe_join 是否考虑矩形边角的遮挡。一般需要考虑。
 * \return  优化后路径大小
 */
int min_dis_opt(const std::vector<std::vector<char> > &v_mat,
				const std::vector<int> &rx,
				const std::vector<int> &ry,
				std::vector<int> *cx,
				std::vector<int> *cy,
				std::vector<int> *pidx,
				bool safe_join)
{
	const int rows = v_mat.size();
	const int cols = v_mat[0].size();

	std::vector<int> lx = rx, ly = ry;
	std::set<int> important;

	size_t test_begin = 0;
	//本次最差的目标
	int opt_tar = test_begin + 2;
	important.insert(test_begin);
	while (test_begin < lx.size() - 2)
	{
		int test_cur = test_begin + 2;
		const int pns = lx.size();
		bool good = true;
		while (test_cur < pns && good)
		{
			const int x1 = lx[test_begin], y1 = ly[test_begin], x2 = lx[test_cur],
					  y2 = ly[test_cur];
			const int dx1 = (x2 - x1), dy1 = (y2 - y1);
			const int absx = dx1 >= 0 ? dx1 : -dx1, absy = dy1 >= 0 ? dy1 : -dy1;
			const int maxp = (absx + absy) * 3;
			for (int i = 0; i < maxp && good; ++i)
			{
				//为不打擦边球，要求周围1格子也没有障碍才能优化。
				for (int d = 0; d < 5 && good; ++d)
				{
					const int rev_dirt[9][2] = {{0, 0},
												{1, 0},
												{0, 1},
												{-1, 0},
												{0, -1},
												{1, 1},
												{-1, 1},
												{1, -1},
												{-1, -1}};
					const int tx = x1 + (i * dx1) / (maxp - 1) + rev_dirt[d][0];
					const int ty = y1 + (i * dy1) / (maxp - 1) + rev_dirt[d][1];
					if (tx >= cols || tx < 0)
						continue;
					if (ty >= rows || ty < 0)
						continue;
					if (v_mat[ty][tx] == 0)
						good = false;
					if (tx == x1 && ty == y1)
						break;
					if (tx == x2 && ty == y2)
						break;
					if (!safe_join)
						break;
				}
			}
			if (!good)
				break;
			++test_cur;
		}
		if (test_cur > opt_tar)
		{
			important.insert(test_begin);
			std::vector<int> newx, newy;
			for (size_t i = 0; i < test_begin; ++i)
			{
				newx.push_back(lx[i]);
				newy.push_back(ly[i]);
			}
			const int x1 = lx[test_begin], y1 = ly[test_begin], x2 = lx[test_cur - 1],
					  y2 = ly[test_cur - 1];
			const int dx1 = (x2 - x1), dy1 = (y2 - y1);
			const int absx = dx1 >= 0 ? dx1 : -dx1, absy = dy1 >= 0 ? dy1 : -dy1;
			const int maxp = (absx + absy) * 3;
			int last_x = -1, last_y = -1;
			for (int i = 0; i < maxp; ++i)
			{
				const int tx = x1 + (i * dx1) / (maxp - 1);
				const int ty = y1 + (i * dy1) / (maxp - 1);
				if (tx != last_x || ty != last_y)
				{
					last_x = tx;
					last_y = ty;
					assert(tx >= 0 && tx < cols);
					assert(ty >= 0 && ty < rows);
					assert(v_mat[ty][tx]);
					newx.push_back(tx);
					newy.push_back(ty);
				}
			}
			//下次务必从这里开始
			opt_tar = newx.size();

			for (int i = test_cur; i < pns; ++i)
			{
				newx.push_back(lx[i]);
				newy.push_back(ly[i]);
			}
			lx = std::move(newx);
			ly = std::move(newy);
		}
		if (good)
			break;

		++test_begin;
	}
	important.insert(lx.size() - 1);

	*cx = std::move(lx);
	*cy = std::move(ly);

	pidx->clear();
	for (auto p : important)
	{
		pidx->push_back(p);
	}

	return cx->size();
}

/*!
 * \brief min_distance_find 避障路径搜索函数
 * \param v_mat  障碍地形，1是平地，0是墙
 * \param startx 起点x
 * \param starty 起点y
 * \param endx   终点x
 * \param endy   终点y
 * \param x      路径x
 * \param y      路径y
 * \param pidx   关键waypoint下表(相对于cx,cy)，连接两个下标的cx,cy的为直线。
 * \param join   是否做直线归并优化。
 * \param safe_join 是否考虑直线边缘的碰撞
 * \param max_itertimers 直线归并最大迭代次数
 * \return cx大小
 */
int min_distance_find(const std::vector<std::vector<char> > &v_mat,
					  const int startx,
					  const int starty,
					  const int endx,
					  const int endy,
					  std::vector<int> *x,
					  std::vector<int> *y,
					  std::vector<int> *pidx,
					  bool join,
					  bool safe_join,
					  int max_itertimers)
{
	std::vector<std::vector<unsigned int> > v_rev;

	if (!mdf_rev_fill(v_mat, startx, starty, endx, endy, &v_rev))
		return 0;
	std::vector<int> cx, cy;

	if (!mdf_path_find(v_rev, startx, starty, &cx, &cy))
		return 0;
	if (join)
	{
		bool res = true;
		int iter = 0;
		while (res && iter < max_itertimers)
		{
			++iter;
			res = res && min_dis_opt(v_mat, cx, cy, x, y, pidx, safe_join);
			if (!res)
				break;
			if (cx.size() == x->size())
			{
				bool same = true;
				const size_t szxc = cx.size();
				for (size_t i = 0; i < szxc && same; ++i)
				{
					if ((*x)[i] != cx[i] || (*y)[i] != cy[i])
						same = false;
				}
				if (same)
					break;
			}
			pidx->clear();
			cx = std::move(*x);
			cy = std::move(*y);
		}
		return res ? iter : 0;
	}

	size_t sz = cx.size();
	pidx->clear();
	for (size_t i = 0; i < sz; ++i)
		pidx->push_back(i);
	*x = std::move(cx);
	*y = std::move(cy);
	return x->size();
}
